//
// Created by bobin on 17-11-11.
//

#include "plucker.h"
#include "opencv2/core/core.hpp"

namespace g2o{
    VertexPluckerLine::VertexPluckerLine() {

    }

    bool VertexPluckerLine::read(std::istream &is) {
        Vector6D lv;
        for (int i=0; i<6; i++)
            is >> _estimate[i];
        return true;
    }

    bool VertexPluckerLine::write(std::ostream &os) const {
        Vector6D lv=estimate();
        for (int i=0; i<6; i++){
            os << lv[i] << " ";
        }
        return os.good();

    }


}// namespace g2o


void PnPL_plucker(const std::vector<cv::Point3f>& pts3d, const std::vector<cv::Point2f>& pts2d,
          const std::vector<cv::Vec6f>& lns3d, const std::vector<cv::Vec4f>& lns2d,
          const cv::Mat& K,
          cv::Mat& R, cv::Mat& t)
{
    int npts = pts3d.size();
    int nlns = lns3d.size();

    // init g2o
    g2o::BlockSolver_6_3::LinearSolverType * linearSolver;
    linearSolver= new g2o::LinearSolverCSparse<g2o::BlockSolver_6_3::PoseMatrixType>();
    g2o::BlockSolver_6_3 * solver_ptr = new g2o::BlockSolver_6_3(linearSolver);
    g2o::OptimizationAlgorithmLevenberg* solver = new g2o::OptimizationAlgorithmLevenberg(solver_ptr);
    g2o::SparseOptimizer optimizer;
    // optimizer.setVerbose(true);
    optimizer.setAlgorithm(solver);

    // add vertex for pose
    int vertex_id = 0;
    Eigen::Vector3d trans(0.1,0.1,0.1);
    Eigen::Quaterniond q;
    q.setIdentity();
    g2o::SE3Quat pose(q,trans);
    g2o::VertexSE3Expmap * v_se3 = new g2o::VertexSE3Expmap();
    v_se3->setId(vertex_id++);
    v_se3->setEstimate(pose);
    optimizer.addVertex(v_se3);

    // set camera intrinsic
    cv::Mat _K;
    K.convertTo(_K, CV_64FC1);
    g2o::CameraParameters * cam_params = new g2o::CameraParameters(_K.at<double>(0,0),
                                                                   Eigen::Vector2d(_K.at<double>(0,2),_K.at<double>(1,2)), 0.);
    cam_params->setId(0);
    optimizer.addParameter(cam_params);

    // add vertex for points and edges for projection
    for(int i=0; i<npts; i++)
    {
        const auto& pt = pts3d[i];
        const auto& uv = pts2d[i];
        // printf("%.3f %.3f %.3f %.3f %.3f\n",pt.x, pt.y, pt.z, uv.x,uv.y);

        g2o::VertexSBAPointXYZ * v_p = new g2o::VertexSBAPointXYZ();
        v_p->setId(vertex_id++);
        v_p->setMarginalized(true);
        v_p->setEstimate(Eigen::Vector3d(pt.x, pt.y, pt.z));
        v_p->setFixed(true);
        optimizer.addVertex(v_p);

        g2o::EdgeProjectXYZ2UV * e = new g2o::EdgeProjectXYZ2UV();
        e->setVertex(0, v_p);
        e->setVertex(1, v_se3);
        e->setMeasurement(Eigen::Vector2d(uv.x,uv.y));
        e->information() = Eigen::Matrix2d::Identity();
        e->setRobustKernel(new g2o::RobustKernelHuber);
        e->setParameterId(0, 0);
        optimizer.addEdge(e);
    }

    // add vertex for lines and edges for projection
    for(int i=0; i<0; i++)
    {
        const auto& ln3d = lns3d[i];
        const auto& ln2d = lns2d[i];
        // printf("%.3f %.3f %.3f %.3f %.3f\n",pt.x, pt.y, pt.z, uv.x,uv.y);

        g2o::VertexPluckerLine * v_l = new g2o::VertexPluckerLine();
        v_l->setId(vertex_id++);
        v_l->setMarginalized(true);
        g2o::Vector6D temp;
        g2o::Vector3D start_temp;
        g2o::Vector3D end_temp;

        start_temp<<ln3d[0],ln3d[1],ln3d[2];
        end_temp<<ln3d[3],ln3d[4],ln3d[5];
        g2o::Vector3D tail3 = end_temp - start_temp;
        g2o::Vector3D head3 = start_temp.cross(end_temp);

        temp.head<3>() = head3;
        temp.tail<3>() = tail3;
        v_l->setEstimate(temp);
        v_l->setFixed(true);
        optimizer.addVertex(v_l);

        g2o::EdgePluckerLine * e = new g2o::EdgePluckerLine();
        e->setVertex(0, v_l);
        e->setVertex(1, v_se3);
        e->setMeasurement(Eigen::Vector4d(ln2d[0],ln2d[1],ln2d[2],ln2d[3]));
        e->information() = Eigen::Matrix4d::Identity();
        e->setRobustKernel(new g2o::RobustKernelHuber);
        e->setParameterId(0, 0);
        optimizer.addEdge(e);
    }

    // optimize
    optimizer.initializeOptimization();
    optimizer.optimize(10);
    // output result
    Eigen::MatrixXd T = Eigen::Isometry3d(v_se3->estimate()).matrix();
    R = (cv::Mat_<float>(3,3)<< T(0,0),T(0,1),T(0,2),
            T(1,0),T(1,1),T(1,2),
            T(2,0),T(2,1),T(2,2));
    t = (cv::Mat_<float>(3,1)<<T(0,3),T(1,3),T(2,3));
}

